/*
The MIT License (MIT)

Copyright (c) 2013 Mike Dapiran, Brian May, Richard Pospesel, and Bert Wierenga

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software 
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE.
*/
#include "hhdVector4.h"
#include "hhdMatrix4.h"
#include "hhdArray2D.hpp"
#include "hhdQuaternion.h"

#include <cstring>

#if HHD_PLATFORM_WINDOWS

#include "d3d10_1.h"
#include <xmmintrin.h>
#include <smmintrin.h>
#endif //HHD_PLATFORM_WINDOWS


#include "hhdMath.h"

namespace hhd
{
	Matrix4 (*Matrix4::matrix_multiply)(const Matrix4&, const Matrix4&) = NULL;

	Matrix4::Matrix4()
	{
		memset(_data, 0, sizeof(float) * 16);
		(*this)(0,0) = 1;
		(*this)(1,1) = 1;
		(*this)(2,2) = 1;
		(*this)(3,3) = 1;
	}

	Matrix4::Matrix4(const Matrix4& other)
	{
		memcpy(_data, other._data, 16 * sizeof(float));
	}

	Matrix4::Matrix4(float _00, float _01, float _02, float _03,
					float _10, float _11, float _12, float _13,
					float _20, float _21, float _22, float _23,
					float _30, float _31, float _32, float _33)
	{
		_data[0] = _00;
		_data[1] = _01;
		_data[2] = _02;
		_data[3] = _03;

		_data[4] = _10;
		_data[5] = _11;
		_data[6] = _12;
		_data[7] = _13;

		_data[8] = _20;
		_data[9] = _21;
		_data[10] = _22;
		_data[11] = _23;

		_data[12] = _30;
		_data[13] = _31;
		_data[14] = _32;
		_data[15] = _33;
	}

		
	Matrix4::Matrix4(float* in_values)
	{
		_data[0] = in_values[0];
		_data[1] = in_values[4];
		_data[2] = in_values[8];
		_data[3] = in_values[12];

		_data[4] = in_values[1];
		_data[5] = in_values[5];
		_data[6] = in_values[9];
		_data[7] = in_values[13];

		_data[8] = in_values[2];
		_data[9] = in_values[6];
		_data[10] = in_values[10];
		_data[11] = in_values[14];

		_data[12] = in_values[3];
		_data[13] = in_values[7];
		_data[14] = in_values[11];
		_data[15] = in_values[15];
	}

	Matrix4::Matrix4(const Quaternion& q)
	{
		//just rotation
			
		//s = 2.0 since quaternions are always kept normalized
		float s = 2.0f;

		float xs = s*q._x;
		float ys = s*q._y;
		float zs = s*q._z;

		float wx = q._w*xs;
		float wy = q._w*ys;
		float wz = q._w*zs;

		float xx = q._x*xs;
		float xy = q._x*ys;
		float xz = q._x*zs;

		float yy = q._y*ys;
		float yz = q._y*zs;
		float zz = q._z*zs;

			
		(*this)(0,0) = 1.0f - (yy+zz);
		(*this)(0,1) = xy-wz;
		(*this)(0,2) = xz+wy;

		(*this)(1,0) = xy+wz;
		(*this)(1,1) = 1.0f - (xx+zz);
		(*this)(1,2) = yz-wx;

		(*this)(2,0) = xz-wy;
		(*this)(2,1) = yz+wx;
		(*this)(2,2) = 1.0f - (xx+yy);

		_data[3] = 0.0f;
		_data[7] = 0.0f;
		_data[11] = 0.0f;

		_data[12] = 0.0f;
		_data[13] = 0.0f;
		_data[14] = 0.0f;

		_data[15] = 1.0f;
	}

	Matrix4::~Matrix4()
	{
	}

/*
	float Matrix4::operator()(int i, int j) const
	{
		return _data[4 * i + j];
	}
*/
/*
	float& Matrix4::operator()(int i, int j)
	{
		return _data[4 * i + j];
	}
*/
	Matrix4::operator const float*() const
	{
		return _data;
	}

	Matrix4& Matrix4::operator=(const Matrix4 m)
	{
		memcpy(_data, m._data, sizeof(float)*16);
		return *this;
	}

		

	Matrix4 Matrix4::operator*(const Matrix4& right) const
	{
		return matrix_multiply(*this, right);
	}

	Vector4 operator*(const Matrix4& left, const Vector4& right)
	{

		Vector4 result(0.0, 0.0, 0.0, 0.0);
		for(int i = 0; i < 4; i++)
			for(int k = 0; k < 4; k++)
				result[i] += left(i, k) * right[k];

		return result;
	}

	Matrix4 Matrix4::identity()
	{
		Matrix4 result;
		result(0,0) = result(1,1) = result(2,2) = result(3,3) = 1.0f;
		return result;
	}

	Matrix4 Matrix4::createPerspectiveProjection(const float& in_horizontal_fov,
			const float& in_aspect_ratio,
			const float& in_near_clip,
			const float& in_far_clip)
	{
		Matrix4 result = Matrix4();
		/*
		float y_scale = Math::cot(in_field_of_view_y/2);
		float x_scale = y_scale / in_aspect_ratio;
		*/

		float x_scale = Math::cot(in_horizontal_fov / 2);
		float y_scale = in_aspect_ratio * x_scale;

		result(0, 0) = x_scale;
		result(1, 1) = y_scale;

		result(2, 2) = in_far_clip / (in_far_clip - in_near_clip );
		//result(2, 3) = 1.0f;
		//result(3, 2) = -in_near_clip * (in_far_clip/(in_far_clip- in_near_clip));

		result(3, 2) = 1.0f;
		result(2, 3) = -in_near_clip * (in_far_clip/(in_far_clip- in_near_clip));
			
		result(3,3) = 0;
		return result;
	}

	Matrix4 Matrix4::createOrthographicProjection(const float& width,
			const float& height,
			const float& far_clip,
			const float& near_clip)
	{
		Matrix4 result;

		result(0,0) = 2.0f/width;

		result(1,1) = 2.0f/height;

		result(2,2) = 1.0f/(far_clip-near_clip);
		result(2,3) = near_clip/(near_clip-far_clip);

		return result;
	}

	Matrix4& Matrix4::scale(const Vector3& in_scale)
	{
		//put scale in
		(*this)(0,0) *= in_scale.x;
		(*this)(1,0) *= in_scale.x;
		(*this)(2,0) *= in_scale.x;
			
		(*this)(0,1) *= in_scale.y;
		(*this)(1,1) *= in_scale.y;
		(*this)(2,1) *= in_scale.y;
			
		(*this)(0,2) *= in_scale.z;
		(*this)(1,2) *= in_scale.z;
		(*this)(2,2) *= in_scale.z;
		return *this;
	}

	Matrix4& Matrix4::scale(float x, float y, float z)
	{
		//put scale in
		(*this)(0,0) *= x;
		(*this)(1,0) *= x;
		(*this)(2,0) *= x;
			
		(*this)(0,1) *= y;
		(*this)(1,1) *= y;
		(*this)(2,1) *= y;
			
		(*this)(0,2) *= z;
		(*this)(1,2) *= z;
		(*this)(2,2) *= z;
		return *this;
	}

	Matrix4& Matrix4::scale(float scale)
	{
		//put scale in
		(*this)(0,0) *= scale;
		(*this)(1,0) *= scale;
		(*this)(2,0) *= scale;
										  
		(*this)(0,1) *= scale;
		(*this)(1,1) *= scale;
		(*this)(2,1) *= scale;
										  
		(*this)(0,2) *= scale;
		(*this)(1,2) *= scale;
		(*this)(2,2) *= scale;

		return *this;
	}

	Matrix4 Matrix4::createSRT(const float& scale_x, const float& scale_y, const float& scale_z,
			const float& angle_about_x, const float& angle_about_y, const float& angle_about_z,
			const float& translation_x, const float& translation_y, const float& translation_z)
	{
		Matrix4 result;
		//get rotation in

		//first column
		result(0,0) = Math::cos(angle_about_y) * Math::cos(angle_about_z);
		result(1,0) = Math::cos(angle_about_y) * Math::sin(angle_about_z);
		result(2,0) = -Math::sin(angle_about_y);

		//second column
		result(0,1) = ( -Math::cos(angle_about_x) * Math::sin(angle_about_z)) + (Math::sin(angle_about_x) * Math::sin(angle_about_y) * Math::cos(angle_about_z));
		result(1,1) = (Math::cos(angle_about_x) * Math::cos(angle_about_z)) + (Math::sin(angle_about_x) * Math::sin(angle_about_y) * Math::sin(angle_about_z));
		result(2,1) = Math::sin(angle_about_x) * Math::cos(angle_about_y);

		//third column
		result(0,2) = (Math::sin(angle_about_x) * Math::sin(angle_about_z)) + (Math::cos(angle_about_x) * Math::sin(angle_about_y) * Math::cos(angle_about_z));
		result(1,2) = (-Math::sin(angle_about_x) * Math::cos(angle_about_z)) + (Math::cos(angle_about_x) * Math::sin(angle_about_y) * Math::sin(angle_about_z));
		result(2,2) = Math::cos(angle_about_x) * Math::cos(angle_about_y);

		//put scale in
		result(0,0) = result(0,0) * scale_x;
		result(1,0) = result(1,0) * scale_x;
		result(2,0) = result(2,0) * scale_x;
			
		result(0,1) = result(0,1) * scale_y;
		result(1,1) = result(1,1) * scale_y;
		result(2,1) = result(2,1) * scale_y;
			
		result(0,2) = result(0,2) * scale_z;
		result(1,2) = result(1,2) * scale_z;
		result(2,2) = result(2,2) * scale_z;

		//put in translation
		result(0,3) = translation_x;
		result(1,3) = translation_y;
		result(2,3) = translation_z;

		return result;
	}

	Matrix4 Matrix4::createSRT(float scale_x, float scale_y, float scale_z, const Vector3& orientation, const Vector3& translation)
	{
		return createSRT(scale_x, scale_y, scale_z, orientation.x, orientation.y, orientation.z, translation.x, translation.y, translation.z);
	}

	Matrix4& Matrix4::toIdentity()
	{
		memset(_data, 0, sizeof(float) * 16);
		(*this)(0,0) = (*this)(1,1) = (*this)(2,2) = (*this)(3,3) = 1.0f;
		return *this;
	}

	Matrix4& Matrix4::tozero()
	{
		memset(_data, 0, sizeof(float) * 16);
		return *this;
	}
	bool Matrix4::setRow(int i, float c0, float c1, float c2, float c3)
	{
		if(i < 0 || i > 3)
			return false;

		int index = i * 4;
		_data[index++] = c0;
		_data[index++] = c1;
		_data[index++] = c2;
		_data[index] = c3;

		return true;
	}

	bool Matrix4::setColumn(int j, float c0, float c1, float c2, float c3)
	{
		if(j < 0 || j > 3)
			return false;

		_data[j] = c0;
		_data[4 + j] = c1;
		_data[8 + j] = c2;
		_data[12 + j] = c3;

		return true;
	}

	bool Matrix4::setColumn(int j, const Vector3& col, float r3)
	{
		return setColumn(j, col.x, col.y, col.z, r3);
	}

	Vector4 Matrix4::getColumn(int j) const
	{
		Vector4 result;
			result.x = (*this)(0,j);
			result.y = (*this)(1,j);
			result.z = (*this)(2,j);
			result.W = (*this)(3,j);
		return result;
	}

	Vector4 Matrix4::getRow(int i) const
	{
		Vector4 result;
			result.x = (*this)(i,0);
			result.y = (*this)(i,1);
			result.z = (*this)(i,2);
			result.W = (*this)(i,3);
		return result;
	}

	Vector3 Matrix4::getRotation(float in_prefered_z, ROTATION_OPTION in_option) const
	{
		//http://www.gregslabaugh.name/publications/euler.pdf
		Vector3 result;
		Matrix4 m(*this);
		m.setForward( m.getForward().normalize() );
		m.setUp( m.getUp().normalize() );
		m.setRight( m.getRight().normalize() );
		
		if(in_option == RO_1)
		{
			if( m._data[8] == -1.0f)
			{
				result.z = in_prefered_z;
				result.y = Math::Pi/2.0f;
				result.x = result.z + Math::atan2(m._data[1], m._data[2]);
			}
			else if( m._data[8] == 1.0f )
			{
				result.z = in_prefered_z;
				result.y = Math::Pi/-2.0f;
				result.x = -result.z + Math::atan2(-m._data[1], -m._data[2]);
			}
			else
			{
				result.y = -Math::asin( m._data[8] );
				result.x = Math::atan2( m._data[9]/Math::cos(result.y), m._data[10]/Math::cos(result.y) );
				result.z = Math::atan2(m. _data[4]/Math::cos(result.y), m._data[0]/Math::cos(result.y) );
			}
		}
		else//RO_2
		{
			if( m._data[8] == -1.0f)
			{
				result.z = in_prefered_z;
				result.y = Math::Pi/2.0f;
				result.x = result.z + Math::atan2(m._data[1], m._data[2]);
			}
			else if( m._data[8] == 1.0f )
			{
				result.z = in_prefered_z;
				result.y = Math::Pi/-2.0f;
				result.x = -result.z + Math::atan2(-m._data[1], -m._data[2]);
			}
			else
			{
				result.y = Math::Pi + Math::asin( _data[8] );
				result.x = Math::atan2( m._data[9]/Math::cos(result.y), m._data[10]/Math::cos(result.y) );
				result.z = Math::atan2( m._data[4]/Math::cos(result.y), m._data[0]/Math::cos(result.y) );
			}
		}
		return result;
	}

	Quaternion Matrix4::getRotationQuaternion() const
	{
		return Quaternion(*this);
	}

	Vector3 Matrix4::getForward() const
	{
		return Vector3(_data[2], _data[6], _data[10]).normalize();
	}

	Vector3 Matrix4::getUp() const
	{
		return Vector3(_data[1], _data[5], _data[9]).normalize();
	}

	Vector3 Matrix4::getRight() const
	{
		return Vector3(_data[0], _data[4], _data[8]).normalize();
	}

	Vector3 Matrix4::getScale() const
	{
		Vector3 x_vector((*this)(0,0), (*this)(1,0), (*this)(2,0));
		Vector3 y_vector((*this)(0,1), (*this)(1,1), (*this)(2,1));
		Vector3 z_vector((*this)(0,2), (*this)(1,2), (*this)(2,2));

		return Vector3(x_vector.getMagnitude(), y_vector.getMagnitude(), z_vector.getMagnitude());
	}

	Vector3 Matrix4::getTranslation() const
	{
		return Vector3(_data[3], _data[7], _data[11]);
	}

	void Matrix4::setTranslation(const Vector3& in_translation)
	{
		_data[3] = in_translation.x;
		_data[7] = in_translation.y;
		_data[11] = in_translation.z;
	}

	void Matrix4::setForward(const Vector3& in_forward)
	{
		_data[2] = in_forward.x;
		_data[6] = in_forward.y;
		_data[10] = in_forward.z;
	}

	void Matrix4::setUp(const Vector3& in_up)
	{
		_data[1] = in_up.x;
		_data[5] = in_up.y;
		_data[9] = in_up.z;
	}

	void Matrix4::setRight(const Vector3& in_right)
	{
		_data[0] = in_right.x;
		_data[4] = in_right.y;
		_data[8] = in_right.z;
	}

	Matrix4 Matrix4::getInverse() const
	{
		Matrix4 result = Matrix4::identity();
		//extract scales
		float magForward, magUp, magRight;
		magForward = this->getForward().getMagnitude();
		magUp = this->getUp().getMagnitude();
		magRight = this->getRight().getMagnitude();

		//get basis
		Vector3 forward, up, right;
		forward = this->getForward().normalize();
		up = this->getUp().normalize();
		right = this->getRight().normalize();

		//set basis and scale
		//i
		result(0,0) = right.x/magRight;
		result(0,1) = right.y/magRight;
		result(0,2) = right.z/magRight;
		//j
		result(1,0) = up.x/magUp;
		result(1,1) = up.y/magUp;
		result(1,2) = up.z/magUp;
		//k
		result(2,0) = forward.x/magForward;
		result(2,1) = forward.y/magForward;
		result(2,2) = forward.z/magForward;
			
		//set translation
		Vector3 translation;
		translation.x = -Vector3::dot(this->getTranslation(), right);
		translation.y = -Vector3::dot(this->getTranslation(), up);
		translation.z = -Vector3::dot(this->getTranslation(), forward);

		result.setTranslation(translation);

		//return
		return result;
	}

	/** ripped out from the JAMA library **/

	double hypot(double A, double B)
	{
		return Math::sqrt(A*A + B*B);
	}

	bool Matrix4::getSlowInverse(Matrix4& out_matrix) const
	{
		// first do a QR decomposition
		//Matrix4 QR(*this);
		Array2D<double> QR(4,4);
		for(int i = 0; i < 4; i++)
			for(int j = 0; j < 4; j++)
				QR(i,j) = (*this)(i,j);

		double Rdiag[4];

		for(int k = 0; k < 4; k++)
		{
			double nrm  = 0.0f;
				
			for(int i = k; i < 4; i++)
				nrm = hypot(nrm, QR(i,k));

			if(nrm != 0.0f)
			{
				if(QR(k,k) < 0)
					nrm = -nrm;
				for(int i = k; i < 4; i++)
					QR(i,k) /= nrm;

				QR(k,k) += 1.0f;

				for(int j = k+1; j < 4; j++)
				{
					double s = 0.0f;
					for(int i = k; i < 4; i++)
						s += QR(i,k) * QR(i,j);
					s = -s/QR(k,k);
					for(int i = k; i < 4; i++)
						QR(i,j) += s * QR(i,k);
				}
			}
			Rdiag[k] = -nrm;
		}

		// solve for identity
			
		//Matrix4 X(B);
		Array2D<double> X(4,4);
		for(int i = 0; i < 4; i++)
			for(int j = 0; j < 4; j++)
				X(i,j) = i == j ? 1.0 : 0.0;

		for(int k = 0; k < 4; k++)
		{
			for(int j = 0; j < 4; j++)
			{
				double s = 0.0f;
				for(int i = k; i < 4; i++)
					s += QR(i,k)*X(i,j);
				s = -s/QR(k,k);
				for(int i = k; i < 4; i++)
					X(i,j) += s * QR(i,k);
			}
		}

		for(int k = 3; k >= 0; k--)
		{
			for(int j = 0; j < 4; j++)
				X(k,j) /= Rdiag[k];
			for(int i = 0; i < k; i++)
				for(int j = 0; j < 4; j++)
					X(i,j) -= X(k,j) * QR(i,k);
		}

		for(int i = 0; i < 4; i++)
			for(int j = 0; j < 4; j++)
				out_matrix(i,j) = (float)X(i,j);

		return true;
	}

	PrintStream& operator<<(PrintStream& in_stream, const Matrix4& mat4)
	{
		Vector4 col0(mat4.getColumn(0));
		Vector4 col1(mat4.getColumn(1));
		Vector4 col2(mat4.getColumn(2));
		Vector4 col3(mat4.getColumn(3));
		/*
		in_stream << "<" << col0[0] << "  " << col1[0] << "  " << col2[0] << "  " << col3[0] << endl;
		in_stream << " " << col0[1] << "  " << col1[1] << "  " << col2[1] << "  " << col3[1] << endl;
		in_stream << " " << col0[2] << "  " << col1[2] << "  " << col2[2] << "  " << col3[2] << endl;
		in_stream << " " << col0[3] << "  " << col1[3] << "  " << col2[3] << "  " << col3[3] << ">" << endl;
		**/

		in_stream << "| " << mat4(0,0) << " " << mat4(0,1) << " " << mat4(0,2) << " " << mat4(0,3) << " | " << endl;
		in_stream << "| " << mat4(1,0) << " " << mat4(1,1) << " " << mat4(1,2) << " " << mat4(1,3) << " | " << endl;
		in_stream << "| " << mat4(2,0) << " " << mat4(2,1) << " " << mat4(2,2) << " " << mat4(2,3) << " | " << endl;
		in_stream << "| " << mat4(3,0) << " " << mat4(3,1) << " " << mat4(3,2) << " " << mat4(3,3) << " | " << endl;

		return in_stream;
	}

#if HHD_PLATFORM_WINDOWS
	Matrix4 Matrix4::sse41_multiply(const Matrix4& left, const Matrix4& right)
	{
		Matrix4 result;
		/** dot product each of this rows to rights columns **/
			
		// raw data alignment:
		/*
		0  1  2  3
		4  5  6  7
		8  9  10 11
		12 13 14 15
		*/

		// set the columns ahead of time
		__m128 col0 = _mm_setr_ps(right._data[0], right._data[4], right._data[8], right._data[12]);
		__m128 col1 = _mm_setr_ps(right._data[1], right._data[5], right._data[9], right._data[13]);
		__m128 col2 = _mm_setr_ps(right._data[2], right._data[6], right._data[10], right._data[14]);
		__m128 col3 = _mm_setr_ps(right._data[3], right._data[7], right._data[11], right._data[15]);

		__m128 dest;
		__m128 row;

		/// ROW 0
		row = _mm_loadu_ps(left._data + 0);
			/// COL 0
			dest = _mm_dp_ps(row, col0, 0xF1);
			_mm_store_ss(&result._data[0], dest);
			/// COL 1
			dest = _mm_dp_ps(row, col1, 0xF1);
			_mm_store_ss(&result._data[1], dest);
			/// COL 2
			dest = _mm_dp_ps(row, col2, 0xF1);
			_mm_store_ss(&result._data[2], dest);
			/// COL 3
			dest = _mm_dp_ps(row, col3, 0xF1);
			_mm_store_ss(&result._data[3], dest);
		/// ROW 1
		row = _mm_loadu_ps(left._data + 4);
			/// COL 0
			dest = _mm_dp_ps(row, col0, 0xF1);
			_mm_store_ss(&result._data[4], dest);
			/// COL 1
			dest = _mm_dp_ps(row, col1, 0xF1);
			_mm_store_ss(&result._data[5], dest);
			/// COL 2
			dest = _mm_dp_ps(row, col2, 0xF1);
			_mm_store_ss(&result._data[6], dest);
			/// COL 3
			dest = _mm_dp_ps(row, col3, 0xF1);
			_mm_store_ss(&result._data[7], dest);
		/// ROW 2
		row = _mm_loadu_ps(left._data + 8);
			/// COL 0
			dest = _mm_dp_ps(row, col0, 0xF1);
			_mm_store_ss(&result._data[8], dest);
			/// COL 1
			dest = _mm_dp_ps(row, col1, 0xF1);
			_mm_store_ss(&result._data[9], dest);
			/// COL 2
			dest = _mm_dp_ps(row, col2, 0xF1);
			_mm_store_ss(&result._data[10], dest);
			/// COL 3
			dest = _mm_dp_ps(row, col3, 0xF1);
			_mm_store_ss(&result._data[11], dest);
		/// ROW 3
		row = _mm_loadu_ps(left._data + 12);
			/// COL 0
			dest = _mm_dp_ps(row, col0, 0xF1);
			_mm_store_ss(&result._data[12], dest);
			/// COL 1
			dest = _mm_dp_ps(row, col1, 0xF1);
			_mm_store_ss(&result._data[13], dest);
			/// COL 2
			dest = _mm_dp_ps(row, col2, 0xF1);
			_mm_store_ss(&result._data[14], dest);
			/// COL 3
			dest = _mm_dp_ps(row, col3, 0xF1);
			_mm_store_ss(&result._data[15], dest);

		return result;
	}

	Matrix4 Matrix4::sse3_multiply(const Matrix4& left, const Matrix4& right)
	{
		Matrix4 result;
		/** dot product each of this rows to rights columns **/
			
		// raw data alignment:
		/*
		0  1  2  3
		4  5  6  7
		8  9  10 11
		12 13 14 15
		*/

		// set the columns ahead of time
		__m128 col0 = _mm_setr_ps(right._data[0], right._data[4], right._data[8], right._data[12]);
		__m128 col1 = _mm_setr_ps(right._data[1], right._data[5], right._data[9], right._data[13]);
		__m128 col2 = _mm_setr_ps(right._data[2], right._data[6], right._data[10], right._data[14]);
		__m128 col3 = _mm_setr_ps(right._data[3], right._data[7], right._data[11], right._data[15]);

		__m128 dest;
		__m128 row;

		/// ROW 0
		row = _mm_loadu_ps(left._data + 0);
			/// COL 0
			dest = _mm_mul_ps(row, col0);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[0], dest);
			/// COL 1
			dest = _mm_mul_ps(row, col1);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[1], dest);
			/// COL 2
			dest = _mm_mul_ps(row, col2);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[2], dest);
			/// COL 3
			dest = _mm_mul_ps(row, col3);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);

			_mm_store_ss(&result._data[3], dest);
		/// ROW 1
		row = _mm_loadu_ps(left._data + 4);
			/// COL 0
			dest = _mm_mul_ps(row, col0);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[4], dest);
			/// COL 1
			dest = _mm_mul_ps(row, col1);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[5], dest);
			/// COL 2
			dest = _mm_mul_ps(row, col2);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[6], dest);
			/// COL 3
			dest = _mm_mul_ps(row, col3);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[7], dest);
		/// ROW 2
		row = _mm_loadu_ps(left._data + 8);
			/// COL 0
			dest = _mm_mul_ps(row, col0);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[8], dest);
			/// COL 1
			dest = _mm_mul_ps(row, col1);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[9], dest);
			/// COL 2
			dest = _mm_mul_ps(row, col2);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[10], dest);
			/// COL 3
			dest = _mm_mul_ps(row, col3);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[11], dest);
		/// ROW 3
		row = _mm_loadu_ps(left._data + 12);
			/// COL 0
			dest = _mm_mul_ps(row, col0);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[12], dest);
			/// COL 1
			dest = _mm_mul_ps(row, col1);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[13], dest);
			/// COL 2
			dest = _mm_mul_ps(row, col2);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[14], dest);
			/// COL 3
			dest = _mm_mul_ps(row, col3);
			dest = _mm_hadd_ps(dest, dest);
			dest = _mm_hadd_ps(dest, dest);
			_mm_store_ss(&result._data[15], dest);

		return result;
	}
#endif //HHD_PLATFORM_WINDOWS

	Matrix4 Matrix4::slow_multiply(const Matrix4& left, const Matrix4& right)
	{
		Matrix4 result;

		for(int i = 0; i < 4; i++)
		{
			for(int j = 0; j < 4; j++)
			{
				float cell = 0.0f;
				for(int k = 0; k < 4; k++)
				{
					cell += left(i,j) * right(j,k);
				}
				result(i,j) = cell;
			}
		}

		return result;
	}
}
